// #include "utilOfDetector.h"
#include <ArmorBoxDetector/findArmorBoxes.h>
#include <Classifier/classifier.h>
#include <Log/output.h>
#include <thread>

using namespace cv;

#define CEIL_UINT(a,b) (a + b - 1)/b;
// 旋转角度 角度制
static inline Point2f rotate(Point2f& p, double angle)
{
    double c = cos(angle / 180.0 * CV_PI);
    double s = sin(angle / 180.0 * CV_PI);
    return Point2f(c * p.x - s * p.y, s * p.x + c * p.y);
}

// 长宽比 
static inline double get_lwrate(const cv::RotatedRect& rect) 
{
     //return rect.size.width / rect.size.height;
    return rect.size.height > rect.size.width ?
        rect.size.height / rect.size.width :
        rect.size.width / rect.size.height;
   
}

// 倾斜角大小（-180 ~ 0）
static inline double get_angle(const cv::RotatedRect& rect)
{
    //return rect.angle;
    return rect.size.height > rect.size.width ? rect.angle - 90.0 : rect.angle;
}

// 有效面积与外接长方形面积之比
static inline double areaRatio(const std::vector<cv::Point>& contour, const cv::RotatedRect& rect) 
{
    return cv::contourArea(contour) / rect.size.area();
}

// 灯条夹角
static inline double get_angle_diff(const RotatedRect& rect_i, const RotatedRect& rect_j)
{
    return abs(get_angle(rect_i) - get_angle(rect_j));
}

// 灯条高度差
static inline double get_height_diff(const RotatedRect& rect_i, const RotatedRect& rect_j) 
{
    return abs(rect_i.center.y - rect_j.center.y);
}

// 灯条组成的平行四边形的锐角的大小
static inline double get_box_angle(const RotatedRect& rect_i, const RotatedRect& rect_j)
{
    double angle1 = (get_angle(rect_i) + get_angle(rect_j)) / 2.0;
    auto dcenter = rect_i.center - rect_j.center;
    double angle2 = atan2(dcenter.y, dcenter.x);
    if (angle2 > 0.0)
    {
        angle2 -= 180.0;
    }

    double angle3 = abs(angle1 - angle2);
    if (angle3 > 90.0)
    {
        angle3 = 180.0 - angle3;
    }
    return angle3;
}

// 灯条组成的矩形的长宽比
static inline double get_box_lwrate(const RotatedRect& rect_i, const RotatedRect& rect_j) 
{
    /*double side_length, rect_length;
    cv::Point2f centers = rect_i.center - rect_j.center;
    side_length = sqrt(centers.ddot(centers));
    rect_length = MAX(MAX(rect_i.size.width, rect_i.size.height), MAX(rect_j.size.width, rect_j.size.height));

    return  side_length / rect_length;*/

    /*Point2f points_i[4], points_j[4];
    rect_i.points(points_i); 
    rect_j.points(points_j);
    vector<Point2f> points = { points_i[0], points_i[1], points_i[2], points_i[3],
                               points_j[0], points_j[1], points_j[2], points_j[3] };
    auto r = cv::minAreaRect(cv::Mat(points));
    return get_lwrate(r);*/

    //Point2f center = (rect_i.center + rect_j.center) / 2;
    double L1 = MAX(MAX(rect_i.size.width, rect_i.size.height), MAX(rect_j.size.width, rect_j.size.height));
    double L2 = sqrt((rect_i.center - rect_j.center).ddot(rect_i.center - rect_j.center));
    return L2 / L1;
}

// 灯条长度之比
static inline double get_length_ratio(const RotatedRect& rect_i, const RotatedRect& rect_j) 
{
    double rect_length_i, rect_length_j;
    rect_length_i = MAX(rect_i.size.width, rect_i.size.height);
    rect_length_j = MAX(rect_j.size.width, rect_j.size.height);
    return MAX(rect_length_i / rect_length_j, rect_length_j / rect_length_i);
}

// 灯条长度差
static inline double get_length_diff(const RotatedRect& rect_i, const RotatedRect& rect_j)
{
    double rect_length_i, rect_length_j;
    rect_length_i = MAX(rect_i.size.width, rect_i.size.height);
    rect_length_j = MAX(rect_j.size.width, rect_j.size.height);
    return abs(rect_length_i - rect_length_j);
}

void bgr2hsvPixel(const uchar bgr[3], int hsv[3])
{
    const int hsv_shift = 12;
    static int sdiv_table[256];
    static int hdiv_table180[256];
    static int hdiv_table256[256];
    static volatile bool initialized = false;

    const int hr = 180;
    const int* hdiv_table = hr == 180 ? hdiv_table180 : hdiv_table256;

    int b = bgr[0], g = bgr[1], r = bgr[2];
    int h, s, v;
    int min;
    int vr, vg;
    int diff;

    if (!initialized)
    {
        sdiv_table[0] = hdiv_table180[0] = hdiv_table256[0] = 0;
        for (int i = 1; i < 256; i++)
        {
            sdiv_table[i] = saturate_cast<int>((255 << hsv_shift) / (1. * i));
            hdiv_table180[i] = saturate_cast<int>((180 << hsv_shift) / (6. * i));
            hdiv_table256[i] = saturate_cast<int>((256 << hsv_shift) / (6. * i));
        }
        initialized = true;
    }
   

    v = MAX(MAX(r, g), b);
    min = MIN(MIN(r, g), b);

    diff = v - min;
    vr = v == r ? -1 : 0;
    vg = v == g ? -1 : 0;

    s = (diff * sdiv_table[v] + (1 << (hsv_shift - 1))) >> hsv_shift;
    h = (vr & (g - b)) +
        (~vr & ((vg & (b - r + 2 * diff)) + ((~vg) & (r - g + 4 * diff))));
    h = (h * hdiv_table[diff] + (1 << (hsv_shift - 1))) >> hsv_shift;
    h += h < 0 ? hr : 0;

    hsv[0] = h;
    hsv[1] = s;
    hsv[2] = v;
}

inline bool ArmorBoxesDetector::isBlue(const uchar hsv[3])
{
    const uchar h = hsv[0], s = hsv[1], v = hsv[2];
    return ((s < max_s2&& v > min_v2) || 
            ( h > min_blue_h && h < max_blue_h && s > min_s && s < max_s && v > min_v));
}

inline bool ArmorBoxesDetector::isRed(const uchar hsv[3])
{
    const uchar h = hsv[0], s = hsv[1], v = hsv[2];
    return ((s < max_s2 && v > min_v2) ||
           ((h > min_red_h || h < max_red_h) && s > min_s && s < max_s && v > min_v));
}

inline bool ArmorBoxesDetector::isBlueBlob(const cv::Mat& src)
{
    auto meanArr = mean(src);
    return meanArr[0] > meanArr[2]*2;
}

inline bool ArmorBoxesDetector::isRedBlob(const cv::Mat& src)
{
    auto meanArr = mean(src);
    return meanArr[2] > meanArr[0]*2;
}

bool ArmorBoxesDetector::isValidLightBlob(const cv::RotatedRect& rect)
{
    //// 以下是各种参数的阈值
    //static const double large_lwrate = 15.0;
    //static const double low_angle = -135.0;
    //static const double large_angle = -45.0;
    //static const double low_area = 20;
    //static const double large_area = 100000;

    bool check1 = get_lwrate(rect) < max_lwrate;
    bool check2 = get_angle(rect) > min_angle && get_angle(rect) < max_angle;
    bool check3 = rect.size.area() > min_area && rect.size.area() < max_area;

    //printf("angle: %f\tlw_rate: %f\tsize: (%f, %f)\tarea: %f\n", get_angle(rect), lw_rate(rect), rect.size.width, rect.size.height, rect.size.area());
    return check1 && check2 && check3;
}

bool ArmorBoxesDetector::isCoupleLightBlobs(const RotatedRect& rect_i, const RotatedRect& rect_j)
{
    double angel_diff = get_angle_diff(rect_i, rect_j);
    double length_radio = get_length_ratio(rect_i, rect_j);
    double length_diff = get_length_diff(rect_i, rect_j);

    /*static const double k0 = 1.5;
    static const double k1 = 3.0;
    static const double large_angle = 30.0;
    static const double low_angle = 60.0;
    static const double max_length_diff = 10.0*/

    bool check1 = angel_diff < max_angle_diff;
    bool check2 = length_radio < max_length_radio || length_diff < max_length_diff;

    if (check1 && check2)
    {
        double box_lwrate = get_box_lwrate(rect_i, rect_j);
        double box_angle = get_box_angle(rect_i, rect_j);

        bool check3 = box_lwrate < max_box_lwrate;       
        bool check4 = box_angle > min_box_angle;
        return check3 && check4;
    }

    //printf("a_diff: %f\t h_diff: %f \t l_radio: %f \t lwrate: %f\n", angel_diff, height_diff, length_radio, lwrate);
    return false;
}

void ArmorBoxesDetector::findBlobs(const Mat& src, vector<RotatedRect>& rst)
{
    //vector<int64>times;

    const cv::Mat kernel_dilate = getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3));
    Mat src_small, src_hsv,img_bin, img_bin2;

    //times.push_back(getTickCount());
    // 第一步：降采样
    if (downsampled_k != 1.0)
    {
        cv::resize(src, src_small, Size(0, 0), 1 / downsampled_k, 1 / downsampled_k);
    }
    else
    {
        src_small = src;
    }
    
    //第二步：转化为HSV
    cvtColor(src_small, src_hsv, COLOR_BGR2HSV);

    img_bin.create(src_hsv.size(), CV_8UC1);
    
    uchar* dstdata = img_bin.data;
    const uchar* srcdata = src_hsv.data;
    bool check;
    int srcoffset = 0, dstoffset = 0, totalpixelnum = src_hsv.rows*src_hsv.cols;

    // 第三步：基于阈值的分割
    // 遍历像素点，利用isEnemyColor进行判断
    for (; dstoffset < totalpixelnum; ++dstoffset, srcoffset+=3)
    {
        if ((this->*isEnemyColor)(srcdata + srcoffset))
        {
            dstdata[dstoffset] = 255;
            continue;
        }
        dstdata[dstoffset] = 0;
    }
    

    //for (int row = 0, rows = src_hsv.rows; row < rows; ++row)
    //{
    //    for (int col = 0, cols = src_hsv.cols; col < cols; ++col)
    //    {
    //        dstoffset = row * cols + col;
    //        srcoffset = dstoffset *3;
    //       
    //        check = (this->*isEnemyColor)(srcdata + srcoffset);
    //        if (check)
    //        {
    //            dstdata[dstoffset] = 255;
    //            continue;
    //        }
    //        dstdata[dstoffset] = 0;

    //        /*if (h > 80 && h < 110 && s > 50 && s < 200 && v > 200)
    //        {
    //            img_bin.at<uchar>(row, col) = 255;
    //        }
    //        else if (s < 25 && v > 240)
    //        {
    //            img_bin.at<uchar>(row, col) = 255;
    //        }
    //        else
    //        {
    //            img_bin.at<uchar>(row, col) = 0;
    //        }*/
    //    }
    //}
   
    // 第四步 膨胀操作，去空洞
    cv::dilate(img_bin, img_bin2, kernel_dilate);
    //times.push_back(getTickCount());

    
    vector<std::vector<cv::Point>> roi_contours;
    std::vector<cv::Vec4i> roi_hierarchy;

    // 第五步 找轮廓
    cv::findContours(img_bin2, roi_contours, roi_hierarchy, RETR_EXTERNAL, CHAIN_APPROX_NONE);
    
    //times.push_back(getTickCount());

    // 第六步 筛选轮廓
    for (int i = 0; i < roi_contours.size(); i++)
    {
        // 判断是否是最外层轮廓，不是的话直接continue
        if (roi_hierarchy[i][2] != -1)
        {
            continue;
        }
        //cv::RotatedRect rect = cv::minAreaRect(light_contours[i]);

        // 找到轮廓在原始输入图像src上对应的位置
        // 由于第一步降采样，现在得到的轮廓坐标和尺寸需要放大downsampled_k倍才是
        // src 上的对应位置与尺寸；同时，我们希望轮廓找的更加精确，先将轮廓尺寸放大enlarge_k
        // 倍，在这个放大的轮廓区域进行自适应阈值分割，找到精确的灯条边缘
        
        auto rect_resized = boundingRect(roi_contours[i]);
        auto rect_src = Rect(rect_resized.tl() * downsampled_k, rect_resized.br() * downsampled_k);
        auto center_src = (rect_src.tl() + rect_src.br()) / 2;
        
        double k = enlarge_k_findBlobs; //放大系数
        auto tl = Point2i(MAX(center_src.x - rect_src.width * k / 2, 0),
                            MAX(center_src.y - rect_src.height * k / 2, 0));
        auto br = Point2i(MIN(center_src.x + rect_src.width * k / 2, src.cols),
                            MIN(center_src.y + rect_src.height * k / 2, src.rows));
        auto rect_roi = Rect(tl, br);

        Mat roi_bgr = src(rect_roi), roi_gray, roi_th;

        // 进一步确定灯条颜色的判断
        if (!(this->*isEnemyBlob)(roi_bgr))
        {
            continue;
        }
         
        cvtColor(roi_bgr, roi_gray, COLOR_BGR2GRAY);
        threshold(roi_gray, roi_th, 0, 255, THRESH_OTSU);

        vector<std::vector<cv::Point>> light_contours;
        std::vector<cv::Vec4i> light_hierarchy;
        cv::findContours(roi_th, light_contours, light_hierarchy, RETR_EXTERNAL, CHAIN_APPROX_NONE);
        //imshow("roi_gray", roi_gray);
        double max_area = -1;
        RotatedRect max_light_rect;
        for (int j = 0; j < light_contours.size(); j++)
        {
            if (light_hierarchy[j][2] != -1)
            {
                continue;
            }
            auto rect_light = cv::minAreaRect(light_contours[j]);
            if (rect_light.size.area() > max_area)
            {
                max_light_rect = rect_light;
                max_area = rect_light.size.area();
            }
        }

        if (isValidLightBlob(max_light_rect))
        {
            max_light_rect.center += (Point2f)rect_roi.tl();
            rst.emplace_back(max_light_rect);
        }
        
    }
   // times.push_back(getTickCount());
   /* for (int i = 1; i < times.size(); ++i)
    {
        auto dt = (times[i] - times[i-1]) / getTickFrequency() * 1000;
        LOG(" No " + to_string(i)+" time is " + to_string(dt) + "ms\n");
    }*/

    if (is_show)
    {
        //// debug 
        //Mat show = src.clone();
        //resize(show, show, Size(0, 0), 0.5, 0.5);
        //resize(show2, show2, Size(0, 0), 0.5, 0.5);
        display("bin", img_bin);
        display("bin2", img_bin2);
        //display("show1", show);
        //cv::imshow("show2", show2);
    }

    ////static int save_idx = 0;
    ////imwrite("D://RM-DATASET//video//battle1//" + to_string(save_idx++) + ".jpg", src);
    //cv::waitKey(1);
}

bool ArmorBoxesDetector::matchArmorBoxes(const cv::Mat& src, const vector<RotatedRect>& light_blobs, vector<ArmorBox>& armor_boxes)
{
    armor_boxes.clear();

    for (int i = 0; i < light_blobs.size(); ++i)
    {
        for (int j = i + 1; j < light_blobs.size(); ++j)
        {
            if (!isCoupleLightBlobs(light_blobs[i], light_blobs[j]))
            {
                continue;
            }
            /*RotatedRect r;
            double angle = (get_angle(light_blobs[i]) + get_angle(light_blobs[j])) / 2.0;
            Point2f center = (light_blobs[i].center + light_blobs[j].center) / 2;
            double L1 = MAX(MAX(light_blobs[i].size.width, light_blobs[i].size.height), MAX(light_blobs[j].size.width, light_blobs[j].size.height));
            double L2 = sqrt((light_blobs[i].center - light_blobs[j].center).ddot(light_blobs[i].center - light_blobs[j].center));
            if (angle <= -90.0)
            {
                angle += 90;
                swap(L1, L2);
            }
            r.angle = angle;
            r.center = center;
            r.size = Size2f(L1, L2);*/
            
            //Point2f points_i[4], points_j[4]; 
            //light_blobs.at(i).points(points_i); // ��ȡ����
            //light_blobs.at(j).points(points_j);
            //vector<Point2f> points = { points_i[0], points_i[1], points_i[2], points_i[3],
            //                           points_j[0], points_j[1], points_j[2], points_j[3] };
            //auto r = cv::minAreaRect(cv::Mat(points));

            //static const double kx = 0.7, ky = 1.8;// x,y �����ı���

            //if (r.angle > -45)
            //{
            //    r.size = Size2f(r.size.width * kx, r.size.height * ky);
            //}
            //else
            //{
            //    r.size = Size2f(r.size.width * ky, r.size.height * kx);
            //}

            armor_boxes.emplace_back(ArmorBox(-1, -1, light_blobs[i], light_blobs[j]));
        }
    }
    return !armor_boxes.empty();
}

ArmorBoxesDetector::ArmorBoxesDetector(int width, int height, const string& modelfile, int maxThreadNum, const string& paramfile):
    width(width), height(height), classifier(modelfile, maxThreadNum)
{
    FileStorage fs(paramfile, FileStorage::READ); //填入读操作
    if (!fs.isOpened())
    {
        cerr << paramfile + "is not opened!!!\n";
        throw runtime_error(paramfile + "is not opened!!!\n");
        return ;
    }
    string str = (string)fs["enemy"];
   
    setEnemy(str);

    downsampled_k = (double)fs["downsampled_k"];
    enlarge_k_findBlobs = (double)fs["enlarge_k_findBlobs"];
    number_window_h = (int)fs["number_window_h"];
    number_window_w = (int)fs["number_window_w"];
    clipping_kh = (double)fs["clipping_kh"];
    clipping_kw = (double)fs["clipping_kw"];

    max_lwrate = (double)fs["max_lwrate"];
    max_angle = (double)fs["max_angle"];
    min_angle = (double)fs["min_angle"];
    min_area = (double)fs["min_area"];
    max_area = (double)fs["max_area"];

    max_length_radio = (double)fs["max_length_radio"];
    max_length_diff = (double)fs["max_length_diff"];
    max_angle_diff = (double)fs["max_angle_diff"];
    max_box_lwrate = (double)fs["max_box_lwrate"];
    min_box_angle = (double)fs["min_box_angle"];

    min_blue_h = (int)fs["min_blue_h"];
    max_blue_h = (int)fs["max_blue_h"];
    min_red_h = (int)fs["min_red_h"];
    max_red_h = (int)fs["max_red_h"];
    min_s = (int)fs["min_s"];
    max_s = (int)fs["max_s"];
    min_v = (int)fs["min_v"];

    max_s2 = (int)fs["max_s2"];
    min_v2 = (int)fs["min_v2"];

    str = (string)fs["ARMORBOXESDETECTOR_SHOW"];
    if (str == "on")
    {
        set_show(true);
    }

    str = (string)fs["CLASSIFIER_SHOW"];
    if (str == "on")
    {
        classifier.set_show(true);
    }
}

void ArmorBoxesDetector::operator()(const Mat& src, vector<ArmorBox>& rst, vector<RotatedRect>& blobs, vector<ArmorBox>& suspects)
{
    //printf("flag11\n");
    //vector<RotatedRect> blobs;
    //vector<ArmorBox> armorboxes;
    vector<Mat> rois;
    //vector<ArmorBox> rst;
    rst.clear();
    blobs.clear();
    suspects.clear();

    auto t1 = getTickCount();
    findBlobs(src, blobs);
    //cout << blobs.size() << endl;
    auto t2 = getTickCount();
    auto dt = (t2 - t1) / getTickFrequency() * 1000;
    // LOG(" find_blobs time is " + to_string(dt) + "ms\n");
    //printf("flag12\n");

    if (blobs.empty()) return;

    //return rst;

    matchArmorBoxes(src, blobs, suspects);
    if (suspects.empty()) return;
    //return;
    //printf("flag13\n");

    // for (auto &box : suspects)
    // {
    //     auto src_vertices = box.vertices;
    //     Point2f dst_vertices[4];
    //     Mat roi;
    //     Mat rectImg;

    //     /*static double k_wh = 1.0;
    //     double h = MAX(MAX(box.light_blob_left.size.width, box.light_blob_left.size.height),
    //                 MAX(box.light_blob_right.size.width, box.light_blob_right.size.height)) * 2;
    //     double w = h * k_wh;*/
    //     double w = number_window_w, h = number_window_h;

    //     dst_vertices[0] = Point2f(0, h-1);
    //     dst_vertices[1] = Point2f(0, 0);
    //     dst_vertices[2] = Point2f(w-1, 0);
    //     dst_vertices[3] = Point2f(w-1, h-1);

    //     Mat warpMatrix = getPerspectiveTransform(src_vertices, dst_vertices);
       
    //     cv::warpPerspective(src, rectImg, warpMatrix, Size(w,h));

    //     double kw = clipping_kw;
    //     double kh = clipping_kh;
    //     int xs = w * (1 - kw) / 2;
    //     int xe = w * (1 + kw) / 2;
    //     int ys = h * (1 - kh) / 2;
    //     int ye = h * (1 + kh) / 2;

    //     roi = rectImg(Rect2i(Point(xs, ys), Point(xe, ye)));

    //     rois.emplace_back(roi);
    // }


    // auto classify_rst = classifier(rois); //TOOD

    for (int i = 0, end = suspects.size(); i < end; i++)
    {
        // suspects[i].number = classify_rst[i];
        suspects[i].number  = 1 ;
        rst.emplace_back(suspects[i]);
    }
    //printf("flag14\n");

    //return rst;
}

void ArmorBoxesDetector::draw(const Mat& src, Mat &dst, const vector<ArmorBox> &rst, const vector<RotatedRect> &blobs, const vector<ArmorBox> &suspects)
{
    static int imgIdx = 0;
    dst = src.clone();
    static auto yellow = Scalar(0,255,255);
    static auto purple = Scalar(255,0,128);
    static auto green = Scalar(0,255,0);
    static auto blue = Scalar(255, 0, 0);
    static auto red = Scalar(0, 0, 255);

    // 画可能的灯条
    for(auto & b : blobs)
    {
        ellipse(dst, b, yellow, FILLED);
    }

    // 画初步识别的装甲板
    for(auto & box : suspects)
    {
        auto vertices = box.vertices;
        for (int i = 0; i < 4; i++)
        {
            cv::line(dst, vertices[i], vertices[(i + 1) % 4], blue, 2, LINE_AA);
        }
    }
    // 画真实装甲板
    for (auto & box : rst)
    {
        auto n = box.number;
        if (n == 0)
            continue;
        auto vertices = box.vertices;
        for (int i = 0; i < 4; i++)
        {
            cv::line(dst, vertices[i], vertices[(i + 1) % 4], purple, 2, LINE_AA);
        }
        ellipse(dst, box.light_blob_left, green,  FILLED);
        ellipse(dst, box.light_blob_right, green, FILLED);
        cv::putText(dst, to_string(n), vertices[0], FONT_HERSHEY_COMPLEX, 1, red);
    }          
    cv::putText(dst, to_string(imgIdx++), Point(40, 40), FONT_HERSHEY_COMPLEX, 1, green);
    resize(dst, dst, Size(0, 0), 0.8, 0.8);
}
/*
 *   @brief: Select the most relevant armorBox from all armorBoxes and return it.
 *   Logic: use the distance between armorBoxes detected in current frame with the center of the target armorBox of the last frame, choose the minimum.
 *   Error Tolerance logic: when frame drops, errorToleranceCounter +=1, if errorToleranceCounter <= errorToleranceThresh, tolerate the error.
 */
ArmorBox ArmorBoxesDetector::SelectArmorBoxesWithMaxHeight(const vector<ArmorBox> &rst) {
    auto selectedArmorBox = rst[0];
    double maxHeight = -1;
    for (auto &r : rst)
    {
        // Calculate the distance between r.center with targetCenter(center of the selected armor box of the last frame.  )
        auto h = r.getHeight();
        if (h > maxHeight)
        {
            maxHeight = h;
            selectedArmorBox = r;
        }
    }
    return selectedArmorBox;
}

void ArmorBoxesDetector::setEnemy(const string& str)
{
    if (str == "blue")
    {
        enemy = Enemy::Blue;
        isEnemyColor = &ArmorBoxesDetector::isBlue;
        isEnemyBlob = &ArmorBoxesDetector::isBlueBlob;
    }
    else if (str == "red")
    {
        enemy = Enemy::Red;
        isEnemyColor = &ArmorBoxesDetector::isRed;
        isEnemyBlob = &ArmorBoxesDetector::isRedBlob;
    }
    else
    {
        throw "load enemy error";
    }
}

double ArmorBox::getHeight() const
{
    double approximate_height = (norm(vertices[0] - vertices[1]) + norm(vertices[2] - vertices[3])) / 2.0;
    return approximate_height;
}

double ArmorBox::getWidth() const
{
    double approximate_width = (norm(vertices[0] - vertices[3]) + norm(vertices[1] - vertices[2])) / 2.0;
    return approximate_width;
}

pair<ArmorBox, double> ArmorBoxesDetector::SelectArmorBoxesWithMaxCorrelation(const vector<ArmorBox> &rst,
                                                                const Point2f &targetCenter) {
    auto selectedArmorBox = rst[0];
    double minDist = -1.0;
    for (auto &r : rst)
    {
        // Calculate the distance between r.center with targetCenter(center of the selected armor box of the last frame.  )
        auto dist = norm(r.getCenter() - targetCenter);
        if (minDist < 0.0 || dist < minDist)
        {
            minDist = dist;
            selectedArmorBox = r;
        }
    }
    return {selectedArmorBox, minDist};
}

void ArmorBoxesDetector::SelectArmorBoxesDepthFilter(vector<ArmorBox>& rst, Camera& cam)
{
    const double depth_thresh = 8000.0; 
    for(auto it= rst.begin(); it != rst.end();)
    {
        double depth = cam.getDistance(ARMORBOX_SMALL, (*it).vertices);
        if(depth > depth_thresh)
        {
            it = rst.erase(it);
        }
        else
        {
            it++;
        }
        
    }
}

int ArmorBoxesDetector::getDetectState() const 
{
    return detectState; 
}

ArmorBox ArmorBoxesDetector::SelectArmorBoxes(vector<ArmorBox> &rst, ArmorBox &lastArmorBox, Camera& cam)
{
    static int errorCounter = 0;
    // 0: target armorBox not detected at last frame, 1: target armorBox detected at last frame
    static int state = 0;

    const int errorToleranceThresh = 20;
    const double distThresh = 200; // This distance is pixel distance

    ArmorBox selectedArmorBox = lastArmorBox; // TODO: Check the coordinate of armorBox.getCenter()

    SelectArmorBoxesDepthFilter(rst, cam);
    // action according to state
    if (1 == state)
    {
        if (rst.empty())
        {
            errorCounter += 1;
            selectedArmorBox = lastArmorBox; // keep selecting lastArmorBox

            if(errorCounter > errorToleranceThresh)
            {
                state = 0;
                errorCounter = 0;
                (selectedArmorBox = ArmorBox()).setNull();
                lastArmorBox = selectedArmorBox;
            }
        }
        else
        { // !rst.empty()
            if(!lastArmorBox.isNull()){
                Point2f targetCenter = lastArmorBox.getCenter();
                auto selected =  SelectArmorBoxesWithMaxCorrelation(rst, targetCenter);
                selectedArmorBox = selected.first;
                double minDist = selected.second;

                if (minDist > distThresh) // new armorBox detected is out of ROI and the target center of the last armorBox is not null.
                {
                    errorCounter += 1;
                    if(errorCounter <= errorToleranceThresh)
                        selectedArmorBox = lastArmorBox;
                        // keep lastArmorBox
                    else // errorCounter > errorToleranceThresh
                    {
                        state = 0;
                        errorCounter = 0;
                        lastArmorBox = selectedArmorBox; // update lastArmorBox
                        // selectedArmorBox = r;
                    }
                }
                else // minDist <= distThresh
                {
                    errorCounter = 0;
                    lastArmorBox = selectedArmorBox; // update lastArmorBox
                    // selectedArmorBox = r;
                }
            }
            else if(lastArmorBox.isNull()) // Choose the armorBox with Biggest height
            {
               selectedArmorBox = SelectArmorBoxesWithMaxHeight(rst);
//               state = 1;
               errorCounter = 0;
               lastArmorBox = selectedArmorBox;
            }
        }
    }
    else if (0 == state)
    {
        if(!rst.empty() && lastArmorBox.isNull())
        {
            state = 1;
            selectedArmorBox = SelectArmorBoxesWithMaxHeight(rst);
            lastArmorBox = selectedArmorBox;
            errorCounter = 0;
        }
        else if (!rst.empty() && !lastArmorBox.isNull())
        {
            state = 1;
            selectedArmorBox = lastArmorBox;
            errorCounter = 0; 
        }
        else // rst.empty()
        {
            state = 0;
            errorCounter = 0;
            (selectedArmorBox = ArmorBox()).setNull(); // set selectedArmorBox to null
            lastArmorBox = selectedArmorBox;
        }
    }

    detectState = state; 
    return selectedArmorBox;
}

void ArmorBoxesDetector::set_show(bool flag)
{
    is_show = flag;
}
/*===================================================================================================*/

/*===================================================================================================*/

ArmorBox::ArmorBox()
{
    color = -1;
    number = -1;
    for (auto &vertex : vertices)
    {
        vertex = {-1.0, -1.0};
    }
    light_blob_left = RotatedRect();
    light_blob_right = RotatedRect();
}

ArmorBox::ArmorBox(int color, int number, const RotatedRect &lb1, const RotatedRect &lb2) : color(color), number(number)
{
    static const double k = 2.2;

    if (lb1.center.x < lb2.center.x)
    {
        light_blob_left = lb1;
        light_blob_right = lb2;
    }
    else
    {
        light_blob_left = lb2;
        light_blob_right = lb1;
    }

    double angle, L;
    Point2f dp;

    // 左灯条
    angle = get_angle(light_blob_left);
    L = MAX(light_blob_left.size.width, light_blob_left.size.height);
    dp = L / 2.0 * Point2f(cos(angle / 180.0 * CV_PI), sin(angle / 180.0 * CV_PI));
    vertices[0] = light_blob_left.center - dp * k;
    vertices[1] = light_blob_left.center + dp * k;

    // 右灯条
    angle = get_angle(light_blob_right);
    L = MAX(light_blob_right.size.width, light_blob_right.size.height);
    dp = L / 2.0 * Point2f(cos(angle / 180.0 * CV_PI), sin(angle / 180.0 * CV_PI));
    vertices[2] = light_blob_right.center + dp * k;
    vertices[3] = light_blob_right.center - dp * k;
}

Point2f ArmorBox::getCenter() const {
    Point2f center = (vertices[0] + vertices[1] + vertices[2] + vertices[3]) / 4.0;
    return center;
}

ArmorBoxType ArmorBox::getArmorBoxType() const {
    //因为ArmorBox现在得到的是一个四边形，不一定是矩形。因此下面是对装甲板宽度和高度的粗略估计
    double approximate_height = (norm(vertices[0] - vertices[1]) + norm(vertices[2] - vertices[3])) / 2.0;
    double approximate_width = (norm(vertices[0] - vertices[3]) + norm(vertices[1] - vertices[2])) / 2.0;
    auto w_h_ratio = approximate_width / approximate_height;

    if(w_h_ratio >= ARMORBOX_TYPE_THRESHOLD)
        return ARMORBOX_BIG;
    else
        return ARMORBOX_SMALL;
}

void ArmorBox::setNull()  {
    number = -1;
}

bool ArmorBox::isNull() const {
    if(number < 0)
        return true;
    else
        return false;
}

